#include "headfiles.h"

int Encoder_Count1 = 0; //编码器1的数值
int Encoder_Count2 = 0; //编码器2的数值

int Speed_Now1, Speed_Now2;

void Motor_Init(void)
{
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
}

// Speed在0~50000之间
// PWMA->PA2
// TIM通道：TIM2 CH3
void MotorLeft_SetSpeed(int32_t Speed)
{
    if (Speed > 0)
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, Speed / 500);
    }
    else if (Speed == 0)
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, Speed / 500);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, -Speed / 500);
    }
}

// Speed在0~50000之间
// PWMB->PA3
// TIM通道：TIM2 CH4
void MotorRight_SetSpeed(int32_t Speed)
{
    if (Speed > 0)
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, Speed / 500);
    }
    else if (Speed == 0)
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, Speed / 500);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, -Speed / 500);
    }
}

//小车动作（前进后退等），Speed的值在0和50000之间：
void car_forward(int32_t Speed)
{
    MotorLeft_SetSpeed(Speed);
    MotorRight_SetSpeed(Speed);
}

void car_backward(int32_t Speed)
{
    MotorLeft_SetSpeed(-Speed);
    MotorRight_SetSpeed(-Speed);
}

void car_left(int32_t Speed)
{
    MotorLeft_SetSpeed(0);
    MotorRight_SetSpeed(Speed);
}

void car_right(int32_t Speed)
{
    MotorLeft_SetSpeed(Speed);
    MotorRight_SetSpeed(0);
}

void car_self_left(int32_t Speed)
{
    MotorLeft_SetSpeed(-Speed);
    MotorRight_SetSpeed(Speed);
}

void car_self_right(int32_t Speed)
{
    MotorLeft_SetSpeed(Speed);
    MotorRight_SetSpeed(-Speed);
}

void car_stop(void)
{
    MotorLeft_SetSpeed(0);
    MotorRight_SetSpeed(0);
}

/*编码器引脚：

E1A->PA0
E1B->PA1

E2A->PB3
E2B->PB4

*/



































